/*
 * GlutConfiguration.cpp
 *
 *  Created on: Jul 5, 2011
 *      Author: sebastien
 */

#include "GlutConfiguration.h"
#define ESCAPE 27
#define X_RESOLUTION 640
#define Y_RESOLUTION 480
#define MAX_DEPTH 5000
#define MIN_DEPTH 100
#define DISPLAY_VIRTUAL_OBJECTS true
#define DISPLAY_VIDEO_IMAGES true
#define DRAW_GHOST_OBJECTS true
#define POSITION_VERBOSE false


int window;
int k = 0;
int users;
BulletConfiguration* bulletConfig;
bool* usersSkeletonLoaded;
CharacterSkeleton** skeletons;

void initGL(int Width, int Height)	        // We call this right after our OpenGL window is created.
{
	glClearColor(0.0f, 0.0f, 0.0f, 0.0f);		// This Will Clear The Background Color To Black
	glClearDepth(1.0);				// Enables Clearing Of The Depth Buffer
	glDepthFunc(GL_LESS);			        // The Type Of Depth Test To Do
	glEnable(GL_DEPTH_TEST);			// Enables Depth Testing
	glEnable(GL_TEXTURE_2D);
	glShadeModel(GL_SMOOTH);			// Enables Smooth Color Shading

	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();				// Reset The Projection Matrix

	gluPerspective(43.0f,(GLfloat)Width/(GLfloat)Height,(GLfloat)MIN_DEPTH,(GLfloat)MAX_DEPTH);	// Calculate The Aspect Ratio Of The Window
	glMatrixMode(GL_MODELVIEW);
}

void drawVideo() {
	xn::ImageGenerator g_image;
	xn::ImageMetaData g_imageMD;
	xn::DepthGenerator g_depth;
	xn::DepthMetaData g_depthMD;

	nRetVal = context.FindExistingNode(XN_NODE_TYPE_IMAGE, g_image);
	checkErrorCode (nRetVal, "Find image node");

	nRetVal = context.FindExistingNode(XN_NODE_TYPE_DEPTH, g_depth);
	checkErrorCode (nRetVal, "Find depth node");

	g_image.GetMetaData(g_imageMD);
	g_depth.GetMetaData(g_depthMD);

	const XnRGB24Pixel* pImageRow = g_imageMD.RGB24Data();

	unsigned int g_nTexMapX = (((unsigned short)(g_depthMD.FullXRes()-1) / 512) + 1) * 512;
	unsigned int g_nTexMapY = (((unsigned short)(g_depthMD.FullYRes()-1) / 512) + 1) * 512;
	XnRGB24Pixel* g_pTexMap = (XnRGB24Pixel*)malloc(g_nTexMapX * g_nTexMapY * sizeof(XnRGB24Pixel));

	XnRGB24Pixel* pTexRow = g_pTexMap + g_imageMD.YOffset() * g_nTexMapX;
	for (XnUInt y = 0; y < g_imageMD.YRes(); ++y)
	{
		const XnRGB24Pixel* pImage = pImageRow;
		XnRGB24Pixel* pTex = pTexRow + g_imageMD.XOffset();

		for (XnUInt x = 0; x < g_imageMD.XRes(); ++x, ++pImage, ++pTex)
		{
			*pTex = *pImage;
		}

		pImageRow += g_imageMD.XRes();
		pTexRow += g_nTexMapX;
	}

	glEnable(GL_TEXTURE_2D);

	glTexParameteri(GL_TEXTURE_2D, GL_GENERATE_MIPMAP_SGIS, GL_TRUE);
	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR_MIPMAP_LINEAR);
	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
	glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, g_nTexMapX, g_nTexMapY, 0, GL_RGB, GL_UNSIGNED_BYTE, g_pTexMap);


	float projectionPlanDistance =-8 * 0.393910476 * X_RESOLUTION; //tan(43/2)

	glLoadIdentity();
	glTranslatef(-X_RESOLUTION * 2,-Y_RESOLUTION * 2,projectionPlanDistance);
	glBegin(GL_QUADS);				// start drawing the projection plan

	glColor4f(1.0f,1.0f,1.0f,1.0f);

	glTexCoord2f(0, 1);
	glVertex3f(0, 0, 0);
	glTexCoord2f(0.66, 1);
	glVertex3f(X_RESOLUTION *4, 0, 0);
	glTexCoord2f(0.66, 0);
	glVertex3f(X_RESOLUTION *4, Y_RESOLUTION *4, 0);
	glTexCoord2f(0, 0);
	glVertex3f(0, Y_RESOLUTION *4, 0);

	glEnd();

	free(g_pTexMap);
	glDisable(GL_TEXTURE_2D);
}

//TODO Virer de ce fichier
void drawBox(btVector3 center, btVector3 halfExtent, float colorsRGBA[3][4]) {
	glLoadIdentity();				// Reset The View
	glTranslatef(center.getX(),center.getY(),-center.getZ());

	glBegin(GL_QUADS);				// start drawing the cube.
	// top of cube
	glColor4f(colorsRGBA[2][0],colorsRGBA[2][1],colorsRGBA[2][2],colorsRGBA[2][3]);
	glVertex3f( halfExtent.getX(), halfExtent.getY(),-halfExtent.getZ());		// Top Right Of The Quad (Top)
	glVertex3f(-halfExtent.getX(), halfExtent.getY(),-halfExtent.getZ());		// Top Left Of The Quad (Top)
	glVertex3f(-halfExtent.getX(), halfExtent.getY(), halfExtent.getZ());		// Bottom Left Of The Quad (Top)
	glVertex3f( halfExtent.getX(), halfExtent.getY(), halfExtent.getZ());		// Bottom Right Of The Quad (Top)
	// bottom of cube
	glVertex3f( halfExtent.getX(),-halfExtent.getY(), halfExtent.getZ());		// Top Right Of The Quad (Bottom)
	glVertex3f(-halfExtent.getX(),-halfExtent.getY(), halfExtent.getZ());		// Top Left Of The Quad (Bottom)
	glVertex3f(-halfExtent.getX(),-halfExtent.getY(),-halfExtent.getZ());		// Bottom Left Of The Quad (Bottom)
	glVertex3f( halfExtent.getX(),-halfExtent.getY(),-halfExtent.getZ());		// Bottom Right Of The Quad (Bottom)

	// front of cube
	glColor4f(colorsRGBA[0][0],colorsRGBA[0][1],colorsRGBA[0][2],colorsRGBA[0][3]);
	glVertex3f( halfExtent.getX(), halfExtent.getY(), halfExtent.getZ());		// Top Right Of The Quad (Front)
	glVertex3f(-halfExtent.getX(), halfExtent.getY(), halfExtent.getZ());		// Top Left Of The Quad (Front)
	glVertex3f(-halfExtent.getX(),-halfExtent.getY(), halfExtent.getZ());		// Bottom Left Of The Quad (Front)
	glVertex3f( halfExtent.getX(),-halfExtent.getY(), halfExtent.getZ());		// Bottom Right Of The Quad (Front)

	// back of cube.
	glVertex3f( halfExtent.getX(),-halfExtent.getY(),-halfExtent.getZ());		// Top Right Of The Quad (Back)
	glVertex3f(-halfExtent.getX(),-halfExtent.getY(),-halfExtent.getZ());		// Top Left Of The Quad (Back)
	glVertex3f(-halfExtent.getX(), halfExtent.getY(),-halfExtent.getZ());		// Bottom Left Of The Quad (Back)
	glVertex3f( halfExtent.getX(), halfExtent.getY(),-halfExtent.getZ());		// Bottom Right Of The Quad (Back)

	// left of cube
	glColor4f(colorsRGBA[1][0],colorsRGBA[1][1],colorsRGBA[1][2],colorsRGBA[1][3]);
	glVertex3f(-halfExtent.getX(), halfExtent.getY(), halfExtent.getZ());		// Top Right Of The Quad (Left)
	glVertex3f(-halfExtent.getX(), halfExtent.getY(),-halfExtent.getZ());		// Top Left Of The Quad (Left)
	glVertex3f(-halfExtent.getX(),-halfExtent.getY(),-halfExtent.getZ());		// Bottom Left Of The Quad (Left)
	glVertex3f(-halfExtent.getX(),-halfExtent.getY(), halfExtent.getZ());		// Bottom Right Of The Quad (Left)

	// Right of cube
	glVertex3f( halfExtent.getX(), halfExtent.getY(),-halfExtent.getZ());	    // Top Right Of The Quad (Right)
	glVertex3f( halfExtent.getX(), halfExtent.getY(), halfExtent.getZ());		// Top Left Of The Quad (Right)
	glVertex3f( halfExtent.getX(),-halfExtent.getY(), halfExtent.getZ());		// Bottom Left Of The Quad (Right)
	glVertex3f( halfExtent.getX(),-halfExtent.getY(),-halfExtent.getZ());		// Bottom Right Of The Quad (Right)
	glEnd();
 }
//TODO put in model
void checkCollisions (BulletConfiguration* bulletConfig,std::list<btCollisionObject*>* collisionsPerUser) {
	//TODO Change to GhostObject Collision detection
	int numManifolds = bulletConfig->getDynamicsWorld()->getDispatcher()->getNumManifolds();

	for (int i=0; i<numManifolds; i++) {
		btPersistentManifold* contactManifold =  bulletConfig->getDynamicsWorld()->getDispatcher()->getManifoldByIndexInternal(i);
		btCollisionObject* obA = static_cast<btCollisionObject*>(contactManifold->getBody0());
		btCollisionObject* obB = static_cast<btCollisionObject*>(contactManifold->getBody1());

		btCollisionObject* partOfBody = 0;
		btCollisionObject* collidedObject = 0;
		if(obA->isKinematicObject() && !obB->isKinematicObject()) {
			partOfBody = obA;
			collidedObject = obB;
		}
		else if(obB->isKinematicObject() && !obA->isKinematicObject()) {
			partOfBody = obB;
			collidedObject = obA;
		}
		if(partOfBody) {
			for (int j = 0; j < users; j++) {
				if(usersSkeletonLoaded[j]) {
					std::list<SkeletonBodyPart>::iterator it;
					std::list<SkeletonBodyPart>* body = (skeletons[j]->getBody());
					for(it=body->begin(); it!=body->end(); it++) {
						if (usersSkeletonLoaded[j] && it->shape == btGhostObject::upcast(partOfBody))
							collisionsPerUser[j].push_back(collidedObject);
					}
				}
			}
		}
	}
}
//TODO Virer de ce fichier ou appeler des methodes externes
void drawGLScene()
{
	nRetVal = context.WaitAndUpdateAll();
	checkErrorCode (nRetVal, "WaitAndUpdateAll");

	glClear(GL_DEPTH_BUFFER_BIT | GL_COLOR_BUFFER_BIT);
	glLoadIdentity();
	if(DISPLAY_VIDEO_IMAGES) {
		drawVideo();
	}
	// Extract hand position of each tracked user

	XnUserID aUsers[users];
	XnUInt16 nUsers = users;

	g_UserGenerator.GetUsers(aUsers, nUsers);
	for (int i = 0; i < nUsers; ++i)
	{
		if (g_UserGenerator.GetSkeletonCap().IsTracking(aUsers[i])) {
			if(!usersSkeletonLoaded[i]) {
				skeletons[i] = new CharacterSkeleton(g_UserGenerator, aUsers[i], bulletConfig);
				//TODO set to false if lost
				usersSkeletonLoaded[i] = true;
			}
			skeletons[i]->update();
		}
	}
	bulletConfig->getDynamicsWorld()->stepSimulation(1.f/30.f,1,1.f/30.f);

	//Display all objects (boxes) with openGL
	//Print positions every 60 frames
	std::list<btCollisionObject*> collisionsPerUser[users];

	checkCollisions(bulletConfig, collisionsPerUser);

	if(DISPLAY_VIRTUAL_OBJECTS) {
		for (int j=bulletConfig->getDynamicsWorld()->getNumCollisionObjects()-1; j>=0 ;j--)
		{
			btCollisionObject* obj = bulletConfig->getDynamicsWorld()->getCollisionObjectArray()[j];
			btRigidBody* body = btRigidBody::upcast(obj);
			if (body && body->getMotionState()){
				btTransform trans;
				if(true || !body->isKinematicObject()) {
					body->getMotionState()->getWorldTransform(trans);
					//TODO Gestion of multi user & colors
					bool collision = false;
					std::list<btCollisionObject*>::iterator it;
					for(int i = 0; i < users && !collision; i++)
						{
							for(it = collisionsPerUser[i].begin(); it != collisionsPerUser[i].end() && !collision; it++)
								collision = collision || (*it == obj);
						}
					float color[3][4];
					if(!collision)
						color = {{1.0f,1.0f,1.0f,1.0f}, {1.0f,0.0f,0.5f,1.0f}, {0.5f,0.0f,1.0f,1.0f}};
					else
						color = {{0.4f,0.4f,0.4f,1.0f}, {0.4f,0.0f,0.2f,1.0f}, {0.2f,0.0f,4.0f,1.0f}};
					if(body->getCollisionShape()->getShapeType() == BOX_SHAPE_PROXYTYPE)
						drawBox(trans.getOrigin(), ((btBoxShape*)body->getCollisionShape())->getHalfExtentsWithoutMargin(), color);
				}
#if POSITION_VERBOSE
				if (k%60 == 0){
				btTransform trans;
				body->getMotionState()->getWorldTransform(trans);
				printf("world pos = %f,%f,%f\n",float(trans.getOrigin().getX()),float(trans.getOrigin().getY()),float(trans.getOrigin().getZ()));
				}
#endif
			} else if (DRAW_GHOST_OBJECTS) {
				if(btGhostObject* gBody = btGhostObject::upcast(obj)) {
					float color[3][4]  = {{0.0f,0.0f,0.0f,1.0f}, {0.1f,0.1f,0.1f,1.0f}, {0.1f,0.1f,0.1f,1.0f}};
					if(gBody->getCollisionShape()->getShapeType() == BOX_SHAPE_PROXYTYPE) {
						btTransform trans = gBody->getWorldTransform();
						drawBox(trans.getOrigin(), ((btBoxShape*)gBody->getCollisionShape())->getHalfExtentsWithoutMargin(), color);
					}
				}
			}
		}
		k++;
	}
	glutSwapBuffers();
}
void keyPressed(unsigned char key, int x, int y)
{
    /* avoid thrashing this call */
    usleep(100);

    /* If escape is pressed, kill everything. */
    if (key == ESCAPE)
    {
      /* shut down our window */
      glutDestroyWindow(window);

      /* exit the program...normal termination. */
      exit(0);
    }
}

void initGlutConfiguration(int nbrUsers, BulletConfiguration* bulletConfiguration, int* pargc, char **argv) {
	users = nbrUsers;
	bulletConfig = bulletConfiguration;
	usersSkeletonLoaded = new bool[users];
	for(int i=0; i<users; i++)
		usersSkeletonLoaded[i] = false;
	skeletons = new CharacterSkeleton*[users];
	printf("Ready\n");
	glutInit(pargc, argv);

	glutInitDisplayMode(GLUT_RGBA | GLUT_DOUBLE | GLUT_ALPHA | GLUT_DEPTH);

	glutInitWindowSize(X_RESOLUTION, Y_RESOLUTION);

	glutInitWindowPosition(0, 0);

	window = glutCreateWindow("Interactions with virtual objects");

	glutDisplayFunc(&drawGLScene);
//	glutFullScreen();
	glutIdleFunc(&drawGLScene);

	//Called when window resized
//	  glutReshapeFunc(&ReSizeGLScene);

	glutKeyboardFunc(&keyPressed);

	initGL(X_RESOLUTION, Y_RESOLUTION);
}


